/*
 * MUL8.cpp
 *
 *  Created on: Jul 2, 2014
 *      Author: unicorn
 */

#include "MUL8.h"
#include "Vision.h"
#include "GameController.h"
#include <pthread.h>

#define MY_TEAM		0
#define MY_TEAM2	1
#define MY_NUMBER	0
#define MY_NUMBER2	1

MotorController mC;
Vision vis;
GameController gameController;
int MUL8_action = 0;
int MUL8_state = -1;

int count = 1;

bool MUL8_initialized = false;
bool MUL8_ready = false;
bool MUL8_set = false;
bool MUL8_play_start = false;
bool MUL8_finished = false;

struct RoboCupGameControlData myData;

MUL8::MUL8() {
	// TODO Auto-generated constructor stub

}

MUL8::~MUL8() {
	// TODO Auto-generated destructor stub
}

double MUL8::getUnixTime() {
	struct timespec tv;

	if (clock_gettime(CLOCK_REALTIME, &tv) != 0) {
		return 0;
	}

	return (((double)tv.tv_sec) + (tv.tv_nsec / 1000000000.0));

}

void MUL8::doMotion() {
	bool done = false;
		double startTime = getUnixTime();
		double currentTime = getUnixTime();
		double stepTime;
		while (!done) {
			int step_result = mC.step(false);
			switch (step_result) {
			case MUL8_STEP_NULL:
	//			vis.grabFrame();
				break;
			case MUL8_STEP_FINISHED:
				// Loop through vision until it's time to execute the next motion
				startTime = getUnixTime();
				currentTime = getUnixTime();
				stepTime = mC.getStepTime();
	//			std::cout << "Step Time: " << stepTime << std::endl;
				stepTime -= 0.15;	// Subtract 150 mS to deal with frame processing time
				do {
					vis.nextFrame();
	//				std::cout << "Step time: " << stepTime << "\t";
	//				std::cout << "Difference between clocks: " << currentTime-startTime << std::endl;
					currentTime = getUnixTime();
					//				std::cout << "Ellapsed time: " << (((float)currentTime-startTime)) << std::endl;
				} while ((currentTime-startTime) < stepTime);
				break;
			case MUL8_MOTION_FINISHED:
				done = true;
				break;
			}
		}
}

void MUL8::search() {
	bool position = false;
//	bool done = false;

	vis.setAction(SEARCH_FOR_GOAL);
	//	vis.setAction(CENTER_BALL);

//	while (!done) {
		if (!position && vis.knowsRobotPosition()) {
			vis.setAction(SEARCH_FOR_BALL);
			std::cout << "Searching for Ball" << std::endl;
			position = true;
		}
		else if (vis.knowsBallPosition()) {
			std::cout << "Searching for both" << std::endl;
			setAction(MUL8_ACTION_WALK_TOWARDS_BALL);
//			done = true;
		}

		waitKey(1);

//		if ((char)c == 27) {
//			done = true;
//		}

		vis.nextFrame();
//	}
	// Robot knows where both the ball and the field are
//	std::cout << "Vision knows where it is, and where the ball is.\n" <<
//			"We would now normally wait for the robot to move," <<
//			" and then update our world." << std::endl;
//	std::cout << std::endl;
//	std::cout << "The robot is located: (" << vis.getRobotX() <<
//			", " << vis.getRobotY() << ") @ " << vis.getRobotTheta()
//			<< std::endl;
//	std::cout << "The ball is located: (" << vis.getBallX() << ", "
//			<< vis.getBallY() << ")" << std::endl;
}
/*
 * This function returns true when it is near the ball, or false if it loses track of the ball.
 */
/*bool*/void MUL8::walkTowardsBall() {
	bool done = false;

		std::cout << "Walking towards the ball time" << std::endl;

		vis.setAction(CENTER_BALL);
		double temp_time = getUnixTime();
	// motorCntroller.turn(vis.getTheta());
		// Search for ball for 6 seconds
		while ((getUnixTime()-temp_time) < 6) {
			vis.nextFrame();
		}
		temp_time = getUnixTime();


//		double last_clock;
		double waitTimer = getUnixTime();
//		last_clock = getUnixTime();
//		bool waitVision = false;
//		int count = 1;
		// TODO I forgot how the robot's X and Y are oriented, might have to change some vision code if this doesn't work
		while (!done) {
			// Toggle between tracking and waiting with Vision
			//		if ((getUnixTime()-waitTimer) > 0.05) {
			//			if (waitVision) {
			//				vis.setAction(WAIT);
			//				waitVision = false;
			//			}
			//			else {
			//				vis.setAction(CENTER_BALL);
			//				waitVision = true;
			//			}
			//			vis.nextFrame();
			//		}
	//		vis.nextFrame();
			//		if (count < vis.getFrequency()) {
			//			count++;
			//			mC.stopHead();
			////			std::cout << "Count: " << count << std::endl;
			//		}
			//		else {
			////			std::cout << "Analyzing Image" << std::endl;
			//			vis.nextFrame();
			//			count = 1;
			//		}
			//		mC.step(false);
			double robotTheta = vis.getRobotTheta();

			double turningTheta = mC.getHeadAngle()*-1;
			double previous_distance = vis.getBallDistance();
			//		std::cout << "Turning theta: " << turningTheta << std::endl;

			// TODO We assume here that each turn left motion turns us approximately 45 degrees
			if (turningTheta > 23) {
				//			vis.nextFrame();
				// FIXME Timer for testing without motions
				//			if ((getUnixTime()-last_clock) > 1) {
				// Need to turn left
	//			if (!mC.isExecuting()) {
					std::cout << "Robot turning left. New theta = " << vis.getRobotTheta() << std::endl;
					if (mC.getMotion() == "W1") {
						mC.setMotion("W1i");
					}
					else if (mC.getMotion() == "W2") {
						mC.setMotion("W2i");
					}
					doMotion();
					//				while (mC.isExecuting()) {
					//					vis.nextFrame();
					//				}
					mC.setMotion("Tl");
					//				dxl_write_word(23, GOAL_POSITION, dxl_read_word(23, PRESENT_POSITION)-100);
					doMotion();
					//				while (mC.isExecuting()) {
					//					vis.nextFrame();
					//				}
					//				mC.setMotion("Wi");
	//				vis.nextFrame();
					//				mC.setMotion("Turn Left");
					vis.updateRobotTheta(45);
					//				last_clock = getUnixTime();
					//				vis.nextFrame();
	//			}
			}
			// TODO We assume here that each turn right motion turns us approximately 45 degrees
			else if (turningTheta < -23) {
				//			vis.nextFrame();
				// FIXME Timer for testing without motions
				// Need to turn right
				//			if ((getUnixTime()-last_clock) > 1) {
	//			if (!mC.isExecuting()) {

					std::cout << "Robot turning right. New theta = " << vis.getRobotTheta() << std::endl;
					if (mC.getMotion() == "W1") {
						mC.setMotion("W1i");
					}
					else if (mC.getMotion() == "W2") {
						mC.setMotion("W2i");
					}
					//				else {
					//					mC.setMotion("Wi");
					//				}
					doMotion();
					//				while (mC.isExecuting()) {
					//					vis.nextFrame();
					//				}
					std::cout << "Setting motion to TR" << std::endl;
					mC.setMotion("Tr");
					//				dxl_write_word(23, GOAL_POSITION, dxl_read_word(23, PRESENT_POSITION)+100);
					doMotion();
					//				while (mC.isExecuting()) {
					//					vis.nextFrame();
					//				}
					//				std::cout << "Motion finished executing" << std::endl;
					//				mC.setMotion("Wi");
	//				vis.nextFrame();
					//				mC.setMotion("Turn Right");
					vis.updateRobotTheta(-45);
					std::cout << "Robot turning right. New theta = " << vis.getRobotTheta() << std::endl;
					//				last_clock = getUnixTime();
	//			}

			}
			// Here we assume that we are facing in the correct direction, so let's walk forward
			else if (previous_distance > 65) { //
				// FIXME Timer for testing without motions
				//			if ((getUnixTime()-last_clock) > 1) {
				//std::cout << "Entered Walking step" << std::endl;
	//			if (!mC.isExecuting()) {
					//				vis.nextFrame();
					std::string currMo = mC.getMotion();
					if (currMo == "Wi") {
						std::cout << "Position W0_m" << std::endl;
						mC.setMotion("W0_m");
						waitTimer = getUnixTime();
					}
					else if (currMo == "W0_m" && ((getUnixTime()-waitTimer) > 1)) {
						mC.setMotion("W2");
						std::cout << "Position W2" << std::endl;
					}
					else if (currMo == "W1") {
						mC.setMotion("W2");
						std::cout << "Position W2" << std::endl;
					}
					else if (currMo == "W2") {
						mC.setMotion("W1");
						std::cout << "Position W1" << std::endl;
					}
					else {
						mC.setMotion("Wi");
						std::cout << "Position Wi" << std::endl;
					}
					doMotion();
					//				mC.setMotion("Walk Forward");
					std::cout << "Robot walking forward. New coordinates: (" << vis.getRobotX() << ", " << vis.getRobotY() << ")" << std::endl;
					std::cout << "Distance to ball: " << previous_distance << std::endl;
					// TODO We assume that the robot moves 3 cm every time it takes a step
					vis.updateRobotPosition(3, robotTheta);
	//			}
				//			if ((getUnixTime()-temp_time) > 0.1) {

				////				vis.nextFrame();
				//				temp_time = getUnixTime();
				//			}
			}
			else {
				// We are within 1 meter of the ball
				//should call getBehindBall();
				// At this point, we would determine how to move, whether to move to the far side of the ball, or walk forward and kick it

				// TODO Add check to make sure robot has finished moving
				//std::cout << "We are close enough to the ball" << std::endl;
	//			if (!mC.isExecuting()) {
					std::string currMo = mC.getMotion();
					if (currMo == "W2") {
						mC.setMotion("W2i");
						std::cout << "Returning from Position W2" << std::endl;
					}
					else if (currMo == "W1") {
						mC.setMotion("W1i");
						std::cout << "Returning from Position W1" << std::endl;
					}

					doMotion();
					//				while (mC.isExecuting()) {}
					done = true;
	//			}
			}
		}
}
/*
 * This function returns true when it is behind the ball, or false if it loses sight of the ball or the ball moves too far away.
 */
/*bool*/void MUL8::getBehindBall() {
//	bool done = false;
	bool turning = false;
	bool turned = false;
	bool walking = false;

	clock_t last_clock = clock();

//	while (!done) {
		vis.nextFrame();
		if ((clock()-last_clock)/CLOCKS_PER_SEC > 1) {
			last_clock = clock();
			double robotTheta = vis.getRobotTheta();

			if (turned) {
				// Walk forward until we're past the ball
				if (!walking) {
					// mC.setMotion("Walk");
					walking = true;
				}
				else if (!mC.isExecuting()) {
					// We need to loop until we pass the ball
					double headTheta = vis.getRobotHeadTheta();
					if (headTheta < -100) {
						// We need to turn left to face the ball
						//mC.setMotion("Turn Left");
					}
					else if (headTheta > 100) {
						// Here we need to turn right to face the ball
						// mC.setMotion("Turn Right");
					}
					else if (/* mC.getMotion() != "Walk" */ false) {
						// Test to see if we need to keep turning
						if (headTheta < 23 && headTheta > -23) {
							// Don't need to keep turning
//							done = true;
						}
						else {
							// Need to keep turning
							// std::string currentMotion = mC.getMotion();
							// mC.setMotion(currentMotion);
						}
					}
					else {
						// mC.setMotion("Walk");
					}
				}
			}


			if (robotTheta < -90) {
				// Robot should turn right to get behind the ball
				std::cout << "Need to turn right to get around ball" << std::endl;
				// After turning, robot should walk forward around 1.5 meters, then turn to face the ball
				if (!turning) {
					// mC.setMotion("Turn Right");
					turning = true;
				}
				else if (!mC.isExecuting()) {
					// FIXME Assuming -45 degrees per right turn
					vis.updateRobotTheta(-45);
					turned = false;
				}
			}
			else if (robotTheta > 90) {
				// Robot should turn left to get behind the ball
				std::cout << "Need to turn left to get around ball" << std::endl;
				// After turning, robot should walk forward around 1.5 meters, then turn to face the ball
				if (!turning) {
					// mC.setMotion("Turn Left");
					// turning = true;
				}
				else if (!mC.isExecuting()) {
					// FIXME Assuming 45 degrees per left turn
					vis.updateRobotTheta(45);
					turned = false;
				}
			}
			else {
				// We are facing in the correct direction, so we need to line up the ball and the goal
				std::cout << "Need to line up ball and goal" << std::endl;
//				done = true;
			}
		}
//	}
}
/*
 * This function returns true if it is ready to kick or false if something else happens
 */
/*bool*/void MUL8::alignToKick() {
	vis.setAction(SEARCH_FOR_BOTH);
}

void MUL8::init() {
	if (!MUL8_initialized) {
		mC.init();
		vis.init(mC);
		mC.setMotion("Wi");
		MUL8_initialized = true;
	}
}

void MUL8::ready() {
	if (!MUL8_ready) {
		std::cout << "I'm ready!" << std::endl;
		MUL8_ready = true;
	}
}

void MUL8::set() {
	if (!MUL8_set) {
		std::cout << "I'm set!" << std::endl;
		MUL8_set = true;
	}
}

void MUL8::play() {
	if (!MUL8_play_start) {
		std::cout << "Let's play!!!!!" << std::endl;
		MUL8_play_start = true;
	}

	setAction(MUL8_ACTION_SEARCH);
	actionStep();
}

void MUL8::penalty() {
	gameController.getGCData(myData);

	if(myData.kickOffTeam == 0){
	std::cout << "Waiting in penalty mode" << std::endl;
	while (myData.teams[MY_TEAM].players[MY_NUMBER].secsTillUnpenalised > 0) {
		gameController.getGCData(myData);
		vis.nextFrame();
		if (myData.teams[MY_TEAM].players[MY_NUMBER].penalty == 0) {
			break;
		}
	}
	}else{
		std::cout << "Waiting in penalty mode" << std::endl;
			while (myData.teams[MY_TEAM2].players[MY_NUMBER2].secsTillUnpenalised > 0) {
				gameController.getGCData(myData);
				vis.nextFrame();
				if (myData.teams[MY_TEAM2].players[MY_NUMBER2].penalty == 0) {
					break;
				}
			}
	}
}

void MUL8::finish() {
	if (!MUL8_finished) {
	std::cout << "Done playing :(" << std::endl;
		MUL8_finished = true;
	}
}

void MUL8::actionStep() {
	switch (MUL8_action) {
	case MUL8_ACTION_SEARCH:
		search();
		break;
	case MUL8_ACTION_WALK_TOWARDS_BALL:
		walkTowardsBall();
		break;
	case MUL8_ACTION_ALIGN_TO_KICK:
		alignToKick();
		break;
	default:
		break;
	}
}

void MUL8::setAction(int new_action) {
	MUL8_action = new_action;
}

void MUL8::setState(int new_state) {
	MUL8_state = new_state;
}

void MUL8::step() {
	gameController.getGCData(myData);

	int MUL8_state = myData.state;
	int penalty_occured = myData.teams[0].players[0].penalty;
	if (penalty_occured == 0) {

		switch(MUL8_state) {
		case MUL8_STATE_INIT:
			init();
			break;
		case MUL8_STATE_READY:
			ready();
			break;
		case MUL8_STATE_SET:
			set();
			break;
		case MUL8_STATE_PLAY:
			play();
			break;
		case MUL8_STATE_FINISH:
			finish();
			break;
		case MUL8_STATE_PENALTY:
			penalty();
			break;
		default:
			break;
		}
	}

	else {
		penalty();
	}
}

